
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_attitude_control_param.c
  * @author     baiyang
  * @date       2021-10-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_attitude_control_multi.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       更新姿态控制器相关参数
  * @param[in]   pAttCtrlMulti  
  * @param[out]  
  * @retval      
  * @note        
  */
void attctrl_update_param(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    //横滚角控制P控制器参数赋值
    att_ctrl->_p_angle_roll._kp =  PARAM_GET_FLOAT(ATT_CTRL, ATC_ANG_RLL_P);

    //俯仰角控制P控制器参数赋值
    att_ctrl->_p_angle_pitch._kp = PARAM_GET_FLOAT(ATT_CTRL, ATC_ANG_PIT_P);

    //偏航角控制P控制器参数赋值
    att_ctrl->_p_angle_yaw._kp = PARAM_GET_FLOAT(ATT_CTRL, ATC_ANG_YAW_P);

    //横滚角速率控制PID控制器参数赋值
    PID_ctrl* pCtrlRPID = attctrl_get_rate_roll_pid(att_ctrl);
    pid_ctrl_set_kp(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_P));
    pid_ctrl_set_ki(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_I));
    pid_ctrl_set_kd(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_D));
    pid_ctrl_set_kimax(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_IMAX));
    pid_ctrl_set_filt_t_hz(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_FLTT));
    pid_ctrl_set_filt_e_hz(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_FLTE));
    pid_ctrl_set_filt_d_hz(pCtrlRPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_RLL_FLTD));

    //俯仰角速率控制PID控制器参数赋值
    PID_ctrl* pCtrlPPID = attctrl_get_rate_pitch_pid(att_ctrl);
    pid_ctrl_set_kp(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_P));
    pid_ctrl_set_ki(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_I));
    pid_ctrl_set_kd(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_D));
    pid_ctrl_set_kimax(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_IMAX));
    pid_ctrl_set_filt_t_hz(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_FLTT));
    pid_ctrl_set_filt_e_hz(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_FLTE));
    pid_ctrl_set_filt_d_hz(pCtrlPPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_PIT_FLTD));

    //偏航角速率控制PID控制器参数赋值
    PID_ctrl* pCtrlYPID = attctrl_get_rate_yaw_pid(att_ctrl);
    pid_ctrl_set_kp(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_P));
    pid_ctrl_set_ki(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_I));
    pid_ctrl_set_kd(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_D));
    pid_ctrl_set_kimax(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_IMAX));
    pid_ctrl_set_filt_t_hz(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_FLTT));
    pid_ctrl_set_filt_e_hz(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_FLTE));
    pid_ctrl_set_filt_d_hz(pCtrlYPID, PARAM_GET_FLOAT(ATT_CTRL, ATC_RAT_YAW_FLTD));

    //设置油门和姿态控制优先级参数
    att_ctrl_multi->_thr_mix_min = PARAM_GET_FLOAT(ATT_CTRL, ATC_THR_MIX_MIN);
    att_ctrl_multi->_thr_mix_man = PARAM_GET_FLOAT(ATT_CTRL, ATC_THR_MIX_MAN);
    att_ctrl_multi->_thr_mix_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_THR_MIX_MAX);

    // 设置姿态角加速度极限值
    att_ctrl->_accel_roll_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_ACCEL_R_MAX);
    att_ctrl->_accel_pitch_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_ACCEL_P_MAX);
    att_ctrl->_accel_yaw_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_ACCEL_Y_MAX);

    // 设置姿态角速度限制值参数
    att_ctrl->_ang_vel_roll_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_RATE_R_MAX);
    att_ctrl->_ang_vel_pitch_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_RATE_P_MAX);
    att_ctrl->_ang_vel_yaw_max = PARAM_GET_FLOAT(ATT_CTRL, ATC_RATE_Y_MAX);

    // 设置角速度前馈参数
    att_ctrl->_rate_bf_ff_enabled = PARAM_GET_INT32(ATT_CTRL, ATC_RATE_FF_EN);

    // 设置角度补偿油门使能参数
    att_ctrl->_angle_boost_enabled = PARAM_GET_INT32(ATT_CTRL, ATC_ANGLE_BOOST);

    // 设置速率控制器输入平滑时间常数参数
    att_ctrl->_input_tc = PARAM_GET_FLOAT(ATT_CTRL, ATC_INPUT_TC);

    // 设置速率控制器输入平滑时间常数参数
    att_ctrl->_angle_limit_tc = PARAM_GET_FLOAT(ATT_CTRL, ATC_ANG_LIM_TC);

    //可以在Loiter，RTL，自动飞行模式下更新偏航目标的最大速率
    att_ctrl->_slew_yaw = PARAM_GET_FLOAT(ATT_CTRL, ATC_SLEW_YAW);

    // 最大角度限制，度*100
    att_ctrl->_angle_max = PARAM_GET_INT16(VEHICLE, ANGLE_MAX);
}

/*------------------------------------test------------------------------------*/


